#include "timemonitor.h"
#include <stdint.h>
#include <string.h>
#include "rtapi/rtapi.h"
static CanCyclicMonitor motion_can_monitor;
int getTa(const double* q, const double* qd, const double* qdd, double* torque) { return 0; }
#include "zuc/nml_intf/zucpos.h"
int kinematicsForward(const ZucPose* toolOffset,
                      const double* joint,
                      ZucPose* world,
                      const KINEMATICS_FORWARD_FLAGS* fflags,
                      KINEMATICS_INVERSE_FLAGS* iflags,
                      const PmRpy* base_offset,
                      const ZucPose* user_offset)
{
    return -1;
}
int kinematicsInverse(const struct ZucPose* toolOffset,
                      const struct ZucPose* world,
                      double* joint,
                      const KINEMATICS_INVERSE_FLAGS* iflags,
                      KINEMATICS_FORWARD_FLAGS* fflags,
                      const PmRpy* base_offset,
                      const ZucPose* user_offset)
{
    return -1;
}
int get_Arm_Angle_from_joint(double* a, double* b) { return -1; }
int update_inverse_flag(const double* joint, const KINEMATICS_FORWARD_FLAGS* fflags, KINEMATICS_INVERSE_FLAGS* iflags) { return -1; }
int kinematicsForward_elbow(const double* joint, ZucPose* world, const PmRpy* base_offset, const ZucPose* user_offset) { return -1; }
void canmonitor_clear()
{
    uint64_t tmp = motion_can_monitor.cur_time;
    memset(&motion_can_monitor, 0, sizeof(motion_can_monitor));
    motion_can_monitor.cur_time = tmp;
    motion_can_monitor.max_cyclic_us = 0;
    motion_can_monitor.min_cyclic_us = UINT64_MAX;
    motion_can_monitor.timer_max_Control = 0;
    motion_can_monitor.timer_max_GetCmdPos = 0;
    motion_can_monitor.timer_max_Cmd = 0;
    motion_can_monitor.timer_max_Can = 0;
    motion_can_monitor.timer_max_SafeAction = 0;
    return;
}

void canmonitor_record_timer(uint8_t chn_id)
{
    return;
    if (chn_id >= MAX_CANCYCLICMONITOR_CNT)
        return;
    uint64_t cur_time = rtapi_get_time() / 1000;
    motion_can_monitor.timer_cur[chn_id] = cur_time;
}

int canmonitor_statistic()
{
    int ret = 0;
    uint64_t cur_time = rtapi_get_time() / 1000;
    uint64_t dt = (rtapi_get_time() / 1000) - motion_can_monitor.cur_time;
    if (dt <= 0)
        return 0;
    motion_can_monitor.cur_cyclic_us = dt;
    motion_can_monitor.cur_time = cur_time;
    if (dt > motion_can_monitor.max_cyclic_us)
    {
        motion_can_monitor.max_cyclic_us = dt;
        memcpy(motion_can_monitor.timer_when_max_cyclic, motion_can_monitor.timer_cur, sizeof(motion_can_monitor.timer_cur));
        ret = 1;
    }
    else if (dt < motion_can_monitor.min_cyclic_us)
    {
        motion_can_monitor.min_cyclic_us = dt;
        memcpy(motion_can_monitor.timer_when_min_cyclic, motion_can_monitor.timer_cur, sizeof(motion_can_monitor.timer_cur));
        ret = 2;
    }

    if (motion_can_monitor.timer_cur[16] - motion_can_monitor.timer_cur[0] > motion_can_monitor.timer_max_Control)
    {
        motion_can_monitor.timer_max_Control = motion_can_monitor.timer_cur[16] - motion_can_monitor.timer_cur[0];
        ret = 3;
    }

    if (motion_can_monitor.timer_cur[10] - motion_can_monitor.timer_cur[9] > motion_can_monitor.timer_max_GetCmdPos)
    {
        motion_can_monitor.timer_max_GetCmdPos = motion_can_monitor.timer_cur[10] - motion_can_monitor.timer_cur[9];
        ret = 4;
    }

    if (motion_can_monitor.timer_cur[49] - motion_can_monitor.timer_cur[48] > motion_can_monitor.timer_max_Cmd)
    {
        motion_can_monitor.timer_max_Cmd = motion_can_monitor.timer_cur[49] - motion_can_monitor.timer_cur[48];
        ret = 5;
    }

    if (motion_can_monitor.timer_cur[20] - motion_can_monitor.timer_cur[17] > motion_can_monitor.timer_max_Can)
    {
        motion_can_monitor.timer_max_Can = motion_can_monitor.timer_cur[20] - motion_can_monitor.timer_cur[17];
        ret = 6;
    }

    if (motion_can_monitor.timer_cur[47] - motion_can_monitor.timer_cur[40] > motion_can_monitor.timer_max_SafeAction)
    {
        motion_can_monitor.timer_max_SafeAction = motion_can_monitor.timer_cur[47] - motion_can_monitor.timer_cur[40];
        ret = 7;
    }

    return ret;
}

void canmonitor_print(int is_sim)
{
    return;
    rtapi_print("[MOTCAN] cycle max:%10lu min:%10lu\n", motion_can_monitor.max_cyclic_us, motion_can_monitor.min_cyclic_us);
    rtapi_print("[MOTCAN] control max:%10lu getcmdPos:%10lu\n", motion_can_monitor.timer_max_Control, motion_can_monitor.timer_max_GetCmdPos);
    rtapi_print("[MOTCAN] cmd max:%10lu can:%10lu\n", motion_can_monitor.timer_max_Cmd, motion_can_monitor.timer_max_Can);
    for (int i = 0; i < MAX_CANCYCLICMONITOR_CNT; i++)
        rtapi_print("[MOTCAN] %2d = %20lu %20lu\n", i, motion_can_monitor.timer_when_max_cyclic[i], motion_can_monitor.timer_when_min_cyclic[i]);
    return;
}